#include "SDLittleDog.h"
#include "SDSlowWalk.h"
#include "SDVectorOp.h"
#include "Classifier.h"
#include <iostream>
#include <fstream>
#include <drawstuff/drawstuff.h>
#include <string>
#include "main.h"
#include <utility>
#include <stack>

#define GOALX 2.75
#define GOALY 0.00
#define d 0.01

using namespace std;

struct step { //the instructions necessary for a single robot step
  public:
     int foot;
     float x;
     float y;

     step(Node* targetNode){
        foot = targetNode->moving_foot;
        x = targetNode->foot_pos[foot].n[0];
        y = targetNode->foot_pos[foot].n[1];
     }
     
};

class smallest_f // comparison function for priority queue
{
public:
  bool operator() (Node& lhs, Node& rhs) const
  {
    printf("using smallest_f\n");
    return (lhs.f > rhs.f);
  }
};

class smallest_node // comparison function for set
{
public:
  bool operator() (Node lhs, Node rhs) const
  {
     //return ( ( abs(lhs[0]-rhs[0]) > d ) || ( abs(lhs[1] - rhs[1]) > d));
     //printf("smallest_node\n");
     for(int i = 0; i < 4; i++){
        //printf("in first for loop\n");

/*         bduVec3f leftF = lhs.foot_pos[i];
           printf("left foot : %f, %f, %f \n", leftF.n[0],leftF.n[1],leftF.n[2]);
           bduVec3f rightF = rhs.foot_pos[i];
           printf("right foot : %f, %f, %f \n", rightF.n[0],rightF.n[1],rightF.n[2]);*/

        for(int j = 0; j < 3; j++){
           //printf("in second for loop\n");


           float left = lhs.foot_pos[i].n[j];
           float right = rhs.foot_pos[i].n[j];

           //printf("abs(left - right) = %f > d = %f && left %f < %f right\n", fabs(left - right), d, left, right);

           if(fabs(left - right) > d && left < right){ 
              //printf(":::: %f < %f\n", left, right);
              return true;
           }
           //printf(":::: %f NOT< %f\n", left, right);
        }
     }
     return false;
  }
};
 
  float EuclidDist(float deltaX, float deltaY) { //Calculates the Euclidian Distance of an ordered pair
      //  printf("delta(x,y) : (%f, %f)\n", deltaX, deltaY);
      //  printf("sqrt(deltaX*deltaX + deltaY*deltaY) = %f\n", sqrt(deltaX*deltaX + deltaY*deltaY));
        return sqrt(deltaX*deltaX + deltaY*deltaY);
  };

  bool GoalTest(Node * testNode) { //determines whether a state/node is above the goal position
       // printf("Testing Goal State of center: (%f, %f)\n", testNode->center[0], testNode->center[1]);
       // printf("Goal state is: (%f, %f)\n", GOALX, GOALY);
        return EuclidDist(testNode->center[0] - GOALX,testNode->center[1]-GOALY) < d;
  };

  
  void FindPath(stack<step>* path, Node* currentNode){ //Recursively backtracks the A* search path
      printf("currentNode address: %d, father address: %d\n", currentNode, currentNode->parent);
      if(currentNode->parent != NULL){
         printf("-------------about to push step of current node.\n");
         path->push(step(currentNode));
         printf("successfully pushed step------------------------\n");
         getchar();
         FindPath(path, currentNode->parent);
      }
  };

  void SavePathToFile(stack<step> * path){
     //NEEDS TO PRINT TO A FILE INSTEAD OF TO THE TERMINAL AND IN THE CORRECT FORMAT TOO
     printf("Found an end state!\n");
     for(int i = 1;!path->empty();i++){
        step nextStep = path->top();
        path->pop();
        printf("Step %d:\n   foot: %d\n   (x,y) = (%f,%f)\n", i, nextStep.foot, nextStep.x, nextStep.y);
     }
  };

  void PrintFootPositions(Node* node){
     printf("\nf: %f\n", node->f);
     printf("center: (%f,%f)\n", node->center[0], node->center[1]);
     bduVec3f* globalFeet = node->foot_pos;
     printf("feet : \n%f, %f, %f \n%f, %f, %f\n%f, %f, %f\n%f, %f, %f\n", globalFeet[0].n[0],globalFeet[0].n[1],globalFeet[0].n[2],globalFeet[1].n[0], globalFeet[1].n[1],globalFeet[1].n[2],globalFeet[2].n[0],globalFeet[2].n[1],globalFeet[2].n[2],globalFeet[3].n[0],globalFeet[3].n[1],globalFeet[3].n[2]);
  }

  float* FindNodeCenter(Node* node){
     float* center = new float[2];
     center[0] = 0;
     center[1] = 0;
     for(int i = 0; i < 4; i++){
        center[0] += node->foot_pos[i].n[0];
        center[1] += node->foot_pos[i].n[1];
     }
     center[0] /= 4;
     center[1] /= 4;
     return center;
  }

  void PrepNodeForDeletion(Node* node){
     delete node->center;
     delete node->foot_pos;
  }

  Node GenerateChildNode(SDSimulator *simulator, Node* current,int i,int j){
     Node child;
     child.foot_pos = new bduVec3f[4]; // pointer on bduVec3f : will be size 4
     for(int k = 0; k < 4; k++){
        if(current->moving_foot != k){
           child.foot_pos[k] = current->foot_pos[k];
        }else{
           float x = current->foot_pos[k].n[0] + i*d;
           float y = current->foot_pos[k].n[1] + j*d;
           child.foot_pos[k].n[0] = x;
           child.foot_pos[k].n[1] = y;
           child.foot_pos[k].n[2] = simulator->getPointHeight(x,y);
        }
     }

     child.moving_foot = (current->moving_foot + 1)%4; //number of the current moving foot
     child.center = FindNodeCenter(&child); //coordinates of the center of the robot : will be size 2 (used identify nodes, to avoid repeated nodes in A*)
     child.h = 4*sqrt(pow(child.center[0]-GOALX,2) + pow(child.center[1]-GOALY,2))/d; // heuristic
     child.g = current->g + 1; // cost
     child.f = child.g + child.h; // f=g+h is used to compare nodes
     child.parent = current;
 
     return child;
  }
  
  bool ValidStep(Node* child/*,Classifier* classifier*/){ //THIS FUNCTION ACTUALLY NEEDS TO BE IMPLEMENTED CORRECTLY
     return true;
  }
   
int main(int argc, char** argv){
  // Initialization
  SDSimulator *simulator;
  SDLittleDog littleDog;
  SDSlowWalk walkController;

  littleDog.setSimulated(true);
  littleDog.setController(&walkController);
  walkController.setLittleDog(&littleDog);
  
  littleDog.initializeSimulator();
  simulator = littleDog.getSimulator();
  
// get the height of a point on the map
  double height = simulator->getPointHeight(0,0);
  printf("height : %f\n",height);
/*
// Create nodes : see main.h for structure "Node"
  struct Node n1;
  n1.f = 1;
  float temp[2]={0,0};
  n1.center = temp;
  printf("node: %f\n", n1.f);

  struct Node n2;
  n2.f = 0;
  n2.center = temp;
  printf("node: %f\n", n2.f);

  priority queue of Nodes, with comparison between nodes made by smallest_f
  priority_queue<Node, vector<Node>, smallest_f> Q; 
  Q.push(n1);
  Q.push(n2);
  struct Node n = Q.top(); // extract node with smallest f
  printf("node: %f\n", n.f);  

// get current foot positions
  bduVec3f globalFeet[4]; // globalFeet is a pointer on a bduVec3f
  simulator->getGlobalFootPositions(globalFeet);
  printf("feet : \n%f, %f, %f \n%f, %f, %f\n%f, %f, %f\n%f, %f, %f\n", globalFeet[0].n[0],globalFeet[0].n[1],globalFeet[0].n[2],globalFeet[1].n[0], globalFeet[1].n[1],globalFeet[1].n[2],globalFeet[2].n[0],globalFeet[2].n[1],globalFeet[2].n[2],globalFeet[3].n[0],globalFeet[3].n[1],globalFeet[3].n[2]);
*/
// Size of grid for foot moving (see handout page 3)
//  int d = 0.01; //1 cm



// For keeping track of expanded Nodes :
// How do we identify nodes? With coordinates of center? With positions of 4 feet? With positions of 4 feet on discretized grid?
// How do we implement it? Set of Nodes, of center positions (=float[2])? (this needs a comparison function, which one?) Tree? how do we implement a tree? make our own class NodeTree, with isPresent(node) function?
// How do we keep track of a node's father, to be able to remember path? Use tree? Add a field "father" to the Node struct?

// set : http://www.cplusplus.com/reference/stl/set/


// set of centers (float[2])
/*  set<float*, smallest_center> S;
  float c[2]={0.2, 4.3};
  S.insert(c);
  
  set<float*, smallest_center>::iterator it;
  float cc[2]={0.2,4.3};
  it = S.find(cc);
  if (it == S.end()){
    printf("not in\n");
  }
  else printf("found : %f, %f\n", (*it)[0], (*it)[1]);
  
  float ccc[2]={0.2,4.299}; // THIS DOESNT WORK : the smallest_center function is wrong
  it = S.find(ccc);
  if (it == S.end()){
    printf("not in\n");
  }
  else printf("found : %f, %f\n", (*it)[0], (*it)[1]);*/



// A* algorithm; Known bugs: I can't figure out how to make the helper functions defined above the main method visible bellow:

  printf("Starting A* search.\n");
  

  printf("Starting Initialization.\n");
  
  // Goal point
  float goal[2] = {2.75,0};

  // Create start node
  bduVec3f globalFeet[4]; // globalFeet is a pointer on a bduVec3f
  simulator->getGlobalFootPositions(globalFeet);
  Node nstart;
  nstart.foot_pos = globalFeet;
  nstart.moving_foot = 0;
  nstart.g = 0;
  float center[2] = {(globalFeet[0].n[0]+globalFeet[1].n[0]+globalFeet[2].n[0]+globalFeet[3].n[0])/4 , (globalFeet[0].n[1]+globalFeet[1].n[1]+globalFeet[2].n[1]+globalFeet[3].n[1])/4};
  nstart.center = center;
  nstart.h = 4*sqrt(pow(center[0]-goal[0],2) + pow(center[1]-goal[1],2))/d; // see handout page 3
  nstart.f = nstart.g + nstart.h;
  nstart.parent = NULL;
  
  //Initialize queue, set, and other variables
  priority_queue<Node, vector<Node>, smallest_f> Q;
  //float GRID_STEP = 0.01; //the width of a cell in the footstep grid
  stack<step> path; //will be used to store retrieved path
  Q.push(nstart);
  PrintFootPositions(&nstart);
  
  
  multiset<Node, smallest_node> expanded;//keep track of expanded nodes
  struct Node current;
  multiset<Node, smallest_node>::iterator it;
  //expanded.insert(nstart);
  //Classifier classifier = new Classifier(0); //NEEDS TO BE DONE CORRECTLY
  

/*  //TEST SIGHT:
     Node node1;
     node1.foot_pos = new bduVec3f[4]; // pointer on bduVec3f : will be size 4

     node1.foot_pos[0].n[0] = 1;
     node1.foot_pos[0].n[1] = 1;
     node1.foot_pos[0].n[2] = 0;
     
     node1.foot_pos[1].n[0] = 1;
     node1.foot_pos[1].n[1] = -1;
     node1.foot_pos[1].n[2] = 0;

     node1.foot_pos[2].n[0] = -1;
     node1.foot_pos[2].n[1] = 1;
     node1.foot_pos[2].n[2] = 0;

     node1.foot_pos[3].n[0] = -1;
     node1.foot_pos[3].n[1] = -1;
     node1.foot_pos[3].n[2] = 0;

     node1.moving_foot = 1; //number of the current moving foot
     node1.center = FindNodeCenter(&node1); //coordinates of the center of the robot : will be size 2 (used identify nodes, to avoid repeated nodes in A*)
     node1.h = .6; // heuristic
     node1.g = .4; // cost
     node1.f = 1.1*nstart.f; // f=g+h is used to compare nodes
     node1.parent = &nstart;
 
     PrintFootPositions(&node1);
*
     Node node2;
     node2.foot_pos = new bduVec3f[4]; // pointer on bduVec3f : will be size 4

     node2.foot_pos[0].n[0] = 3;
     node2.foot_pos[0].n[1] = 1;
     node2.foot_pos[0].n[2] = 0;
     
     node2.foot_pos[1].n[0] = 3;
     node2.foot_pos[1].n[1] = -1;
     node2.foot_pos[1].n[2] = 0;

     node2.foot_pos[2].n[0] = 1;
     node2.foot_pos[2].n[1] = 1;
     node2.foot_pos[2].n[2] = 0;

     node2.foot_pos[3].n[0] = 1;
     node2.foot_pos[3].n[1] = -1;
     node2.foot_pos[3].n[2] = 0;

     node2.moving_foot = 1; //number of the current moving foot
     node2.center = FindNodeCenter(&node2); //coordinates of the center of the robot : will be size 2 (used identify nodes, to avoid repeated nodes in A*)
     node2.h = 0; // heuristic
     node2.g = 1; // cost
     node2.f = 1; // f=g+h is used to compare nodes
     node2.parent = &nstart;
 
     PrintFootPositions(&node2);
     Q.push(node2);printf("pushed node2.\n");
 *   Q.push(nstart);printf("pushed nstart.\n");


     Node node3;
     node3.foot_pos = new bduVec3f[4]; // pointer on bduVec3f : will be size 4

     node3.foot_pos[0].n[0] = 3.75;
     node3.foot_pos[0].n[1] = 1;
     node3.foot_pos[0].n[2] = 0;
     
     node3.foot_pos[1].n[0] = 3.75;
     node3.foot_pos[1].n[1] = -1;
     node3.foot_pos[1].n[2] = 0;

     node3.foot_pos[2].n[0] = 1.73;
     node3.foot_pos[2].n[1] = 1;
     node3.foot_pos[2].n[2] = 0;

     node3.foot_pos[3].n[0] = 1.74;
     node3.foot_pos[3].n[1] = -1;
     node3.foot_pos[3].n[2] = 0;

     node3.moving_foot = 2; //number of the current moving foot
     node3.center = FindNodeCenter(&node3); //coordinates of the center of the robot : will be size 2 (used identify nodes, to avoid repeated nodes in A*)
     node3.h = .1; // heuristic
     node3.g = 1.5; // cost
     node3.f = 1.1; // f=g+h is used to compare nodes
     node3.parent = &node2;
 
     PrintFootPositions(&node3);

     Q.push(node3);
*/


//expanded.insert(node2);expanded.insert(node3); expanded.insert(nstart);


printf("expanded size: %d\n", expanded.size());



  //actual A* algorithm
  
  printf("Starting while loop.\n");

  while(!Q.empty()){

     printf("while loop cycle.\n");

     current = Q.top();
     Q.pop();
     printf("----------Popped the queue-----------Queue size: %d\n", Q.size());
     it=expanded.find(current);
     if(it==expanded.end()){

        printf("Expanding Node.\n");

        if(GoalTest(&current)){

printf("Found Goal state!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");

           printf("Q size = %d, expanded size = %d\n", Q.size(), expanded.size());
           FindPath(&path, &current);
           
           
           printf("Goal is: (%f,%f)\n", GOALX, GOALY);
           PrintFootPositions(&current);

           break;
        }
        expanded.insert(current);
        for(int i = -2; i < 3; i++){
           for(int j = -2; j < 3; j++){
              if(i != 0 && j != 0){
                 Node child = GenerateChildNode(simulator, &current, i, j);

                 if(i == 0 && j == 0){
                 printf("i = f = 0\n");
                 return 1;
}


                 if( (int) child.parent == (int) &child){
                    printf("This node is its own parent!  Q size = %d, expanded size = %d\n", Q.size(), expanded.size());
                    PrintFootPositions(&child);
                    return 1;
                 }

                 if(ValidStep(&child/*,&classifier*/)){
                    Q.push(child);
                    printf("-|-|-|-|-----Pushed the queue--------Queue size: %d\n",Q.size());
                 }
              }
           }
        }     
     }

     //printf("Forced-quit the while loop.\n");
     //break;
  }

  //Save the path (stored in a struct) to a path file

  printf("Starting save-the-path phase.\n");

  if(path.empty()){
     printf("Path not found");
  }else{
     SavePathToFile(&path); //implementation needs to be modified to save to a file.
  }

  //Delete all nodes and classifier (so we aren't orphaning memory)
  
  //delete classifier;
  /*for(set<Node, smallest_node>::iterator it = expanded.begin();it != expanded.end(); it++){
     PrepNodeForDeletion(&(*it));
  }*/
  expanded.clear();
  /*if(!Q.empty()){
     for(Node next = Q.top(); !Q.empty(); next = Q.top()){
        Q.pop();
        PrepNodeForDeletion(&next);
     }
  }*/

  //Exit program
  
  return 0; 
}







